#include "hc_sr04.h"
#include "../hardware/io.h"
#include "sys_timer.h"
#include "../hardware/hal_tim.h"

#define HC_COUNT 4
#define HC_OUTTIME_MS 10
#define SPEED_RATE (340.0 * 100 / 1000000)  // cm/us

uint8_t hc_pins[HC_COUNT][2] = {
    {PB12, PB4},
    {PC1, PA2},
    {PD7, PA3},
    {PB9, PB2},
};

uint8_t hc_distance[HC_COUNT] = {DISTANCE_MAX};
void HcInit() {
    for (int i = 0; i < HC_COUNT; i++) {
        IOInit(hc_pins[i][0], Mode_Out_PP);
        IOInit(hc_pins[i][1], Mode_IN_FLOATING);
    }
    HAL_timer_init(2, 72, 65535);
    HAL_timer_nvic_init(2, 3, 3);
    TIM_DISABLE(2);
}

uint8_t * HcGet() {
    uint32_t s_time;
    uint8_t i = 0, over_count=0;
    uint8_t flag[HC_COUNT] = {0};
    uint16_t h_time[HC_COUNT] = {0};
    TIM_CNT_RESET(2);
    TIM_ENABLE(2);
    for (i = 0; i < HC_COUNT; i++) {
        hc_distance[i] = DISTANCE_MAX;
        IOWrite(hc_pins[i][0], 1);
    }
    s_time = TIM_CNT(2);
    while ((s_time + 15) > TIM_CNT(2));
    for (i = 0; i < HC_COUNT; i++) {
        IOWrite(hc_pins[i][0], 0);
    }
    s_time = CurMillis();
    while ((s_time + HC_OUTTIME_MS) > CurMillis()) {
        for (i = 0; i < HC_COUNT; i++) {
            if (flag[i] == 0) {
                if (IORead(hc_pins[i][1])) {
                    h_time[i] = TIM_CNT(2);
                    flag[i] = 1;
                }
            } else if (flag[i] == 1) {
                if (!IORead(hc_pins[i][1])) {
                    h_time[i] = TIM_CNT(2) - h_time[i];
                    flag[i] = 2;
                    over_count++;
                }
            }
        }
        if (over_count == HC_COUNT)
            break;
    }

    for (i = 0; i < HC_COUNT; i++) {
        if (flag[i] == 2) {
            hc_distance[i] = h_time[i] * SPEED_RATE;
        }
    }

    return hc_distance;
}